from wlkata_mirobot import WlkataMirobot
import time
arm = WlkataMirobot()

P1_cang_Put_up = [-62.54,8.7,-2,0,-6.69,62.54,99.7,-191.82,209.63,0,0,0,0]
P1_cang_Put = [-62.54,9.99,6.34,0,-16.34,62.54,99.72,-191.85,180.76,0,0,0,0]
P_transition = [-77.34,-33.03,31.06,0,1.25,32.09,30.52,-135.93,219.06,-0.38,-0.61,-45.25,0]

P2_cang_Put_up = [-74.39,13.48,-7.05,0,-6.42,74.39,60.54,-216.71,208.7,0,0,0,0]
P2_cang_Put = [-74.9,12.31,3.72,0,-16.03,74.91,57.49,-213.03,180.79,0,0,0,0]

P3_cang_Put_up = [-85.96,12.08,-4.28,0,-7.81,85.95,15.67,-221.88,205.2,0,0,0,0]
P3_cang_Put = [-85.96,13.26,2.67,0,-15.94,85.95,15.68,-221.89,180.7,0,0,0,0]

P4_cang_Put_up = [-97.88,7.66,1.67,0,-9.32,97.88,-29.35,-212.04,202.1,0,0,0,0]
P4_cang_Put = [-97.88,8.94,7.77,0,-16.7,97.88,-29.35,-212.04,180,0,0,0,0]

### 托盘 相对于机器人c从左至右：P1，P2---P3,P4
P1_plate_up = [-9.39,38.71,-40.72,0,2,-36.57,261.86,-43.29,212.9,0,0,-45.95,0]
P1_plate = [-10.27,38.38,-5.19,0,-33.19,-35.68,245.11,-44.4,111.61,0,0,-45.95,0]

P2_plate_up = [-18.88,32.46,-22.72,0,-9.74,-8.52,243.72,-83.37,184.96,0,0,-27.41,0]
P2_plate = [-17.43,40.98,-9.09,0,-31.89,-6,242.87,-76.25,111.99,0,0,-23.43,0]

P3_plate_up = [-13.29,-3.17,24.31,0,-21.15,6.52,183.49,-43.33,168.26,0,0,-6.76,0]
P3_plate = [-10.95,23.4,15.39,0,-38.81,8.2,212.88,-41.19,111.55,0,0,-2.75,0]

P4_plate_up = [-19.83,14.93,7.12,0.12,-22.23,34.36,208.58,-75.17,162.16,0,-0.21,14.48,0]
P4_plate = [-18.91,26.22,11.88,0.16,-38.26,36.45,210.8,-72.14,111.08,0,-0.2,17.45,0]

#### 搬运--至托盘1号位置
#### 过渡点---取料上---取料--取料上---过渡点---托盘上---托盘---托盘上--过渡点--原点
#### 开始取料
arm.set_joint_angle(P = P_transition)
arm.set_joint_angle(P = P1_cang_Put_up)
arm.set_joint_angle(P = P1_cang_Put)
arm.Master.set_DO(0,1)
time.sleep(0.5)
arm.set_joint_angle(P = P1_cang_Put_up)
#### 取料结束

arm.set_joint_angle(P = P_transition)
#### 开始放于支柱托盘1号位置
arm.set_joint_angle(P = P1_plate_up)      
arm.set_joint_angle(P = P1_plate)
arm.Master.set_DO(0,0)
arm.set_joint_angle(P = P1_plate_up)
#### 放于支柱托盘1号位置结束

arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)

#### 搬运--至托盘2号位置
#### 开始取料
arm.set_joint_angle(P = P_transition)
arm.set_joint_angle(P = P2_cang_Put_up)
arm.set_joint_angle(P = P2_cang_Put)
arm.Master.set_DO(0,1)
time.sleep(1) 
arm.set_joint_angle(P = P2_cang_Put_up)
#### 取料结束

arm.set_joint_angle(P = P_transition)

#### 放于支柱托盘2号位置开始
arm.set_joint_angle(P = P2_plate_up)       
arm.set_joint_angle(P = P2_plate)
arm.Master.set_DO(0,0)
time.sleep(1)
arm.set_joint_angle(P = P2_plate_up)
#### 放于支柱托盘2号位置结束
arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)       

#### 搬运--至托盘3号位置
arm.set_joint_angle(P = P_transition)
#### 开始取料
arm.set_joint_angle(P = P3_cang_Put_up)
arm.set_joint_angle(P = P3_cang_Put)
arm.Master.set_DO(0,1)
time.sleep(1) 
arm.set_joint_angle(P = P3_cang_Put_up)
#### 取料结束

arm.set_joint_angle(P = P_transition)
arm.set_joint_angle(P = P3_plate_up)
arm.set_joint_angle(P = P3_plate)
arm.Master.set_DO(0,0)
time.sleep(1)
arm.set_joint_angle(P = P3_plate_up)
arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)       

#### 搬运--至托盘4号位置
arm.set_joint_angle(P = P_transition)
arm.set_joint_angle(P = P4_cang_Put_up)
arm.set_joint_angle(P = P4_cang_Put)
arm.Master.set_DO(0,1)
time.sleep(1)
arm.set_joint_angle(P = P4_cang_Put_up) 
arm.set_joint_angle(P = P_transition)     
arm.set_joint_angle(P = P4_plate_up)
arm.set_joint_angle(P = P4_plate)
arm.Master.set_DO(0,0)
time.sleep(1)
arm.set_joint_angle(P = P4_plate_up)
arm.set_tool_pose(x=198.67,y=0.0, z=230.72,roll=0.0, pitch=0.0,yaw=0.0)